from geometry_msgs.msg import PoseStamped
from nav2_simple_commander.robot_navigator import BasicNavigator#它提供了一个简单的接口来与Nav2系统交互，包括发送导航目标、设置初始位置等
import rclpy
#别忘了我们的消息接口geometry_msgs/msg/PoseWithCovarianceStamped,但是协方差矩阵不需要,就直接扣掉了,剩下一个PoseStamped(居然还能这样)

def main():
    rclpy.init()
    navigator = BasicNavigator()#初始化创建导航器
    initial_pose = PoseStamped()#初始化消息接口
    initial_pose.header.frame_id = 'map'#设置位姿所在的坐标系为map
    initial_pose.header.stamp = navigator.get_clock().now().to_msg()#获取时间戳
    initial_pose.pose.position.x = 0.0
    initial_pose.pose.position.y = 0.0
    initial_pose.pose.orientation.w = 1.0#设置位姿
    navigator.setInitialPose(initial_pose)#将设定的初始位姿传递给导航器
    navigator.waitUntilNav2Active()#此行确保在尝试执行任何导航任务之前，Nav2系统已经完全启动并准备好接受命令
    rclpy.spin(navigator)#让节点保持运行状态，处理传入的消息和服务请求。在这个例子中，由于主要工作是配置初始位姿，所以实际上这里可能不会接收很多消息或服务请求
    rclpy.shutdown()#当不再需要使用ROS 2资源时，清理并关闭所有资源

if __name__ == '__main__':
    main()